#ifndef SWEEPSURF_H_
#include "SweepSurf.h"
#endif

using namespace std;
using std::vector;

SweepSurf :: SweepSurf(GLuint program, vector<float>* Points,vector<float>* PathPoints) : Figure(program) {
 controlPoints = new vector<float>();
 originalControlPoints = new vector<float>();
 controlPoints->insert(controlPoints->end(), Points->begin(), Points->end());
 originalControlPoints->insert(originalControlPoints->end(), Points->begin(), Points->end());

 vectorPerp[0]=0;
 vectorPerp[1]=0;
 vectorPerp[2]=1;

 delta[0]=0;
 delta[1]=0;
 delta[2]=0;

 //BezierCuadCurve aux(PathPoints,vectorPerp);
 BSplineCuadCurve aux(PathPoints,vectorPerp);
 path = new vector<float>();
 aux.getCurve(path); 

 createSurface();
 createVertex();
}

SweepSurf ::~SweepSurf(){
 delete(controlPoints);
 delete(originalControlPoints);
 //delete(normals); //Se borran antes
 //delete(surface); //Se borran antes
 delete(path);
}

float anguloEntreVectores(glm::vec3 vector1, glm::vec3 vector2) {
	//Angulo entre dos vectores.
	float producto = vector1[0]*vector2[0]+vector1[1]*vector2[1]+vector1[2]*vector2[2];
	float modulo1 = sqrt(vector1[0]*vector1[0]+vector1[1]*vector1[1]+vector1[2]*vector1[2]);
	float modulo2 = sqrt(vector2[0]*vector2[0]+vector2[1]*vector2[1]+vector2[2]*vector2[2]);
	float angulo = (producto/(modulo1*modulo2));
	return ((acos(angulo)*180)/3.1415912);
}

void SweepSurf :: createSurface(){

 surface = new vector<float>();
 normals = new vector<float>();

 for(size_t i=3 ; i<path->size(); i+=3){
	
	float dx = (*path)[i]   - (*path)[i-3];
	float dy = (*path)[i+1] - (*path)[i-2];
	float dz = (*path)[i+2] - (*path)[i-1];

	if((i < path->size() - 5) && (i > 2)){
		float dx2 = (*path)[i+3] - (*path)[i];
		float dy2 = (*path)[i+4] - (*path)[i+1];
		float dz2 = (*path)[i+5] - (*path)[i+2];

		float ang = calcularAngulo(0,1,0,dx2,dy2,dz2);

		//cout << "\nd: {" << dx << "," << dy << "," << dz << "}\n";
		//cout << "\nd2: {" << dx2 << "," << dy2 << "," << dz2 << "}\n";
		cout << "\nang: " << ang*(180/3.1415912) << "\n\n";
		rotateCtrlPointsFromOriginal(ang*(180/3.1415912)); // Hay que revisar el eje de rotación.
	}

 	delta[0]+=dx;
 	delta[1]+=dy;
 	delta[2]+=dz;

	vector<float>* control = translate();
 	BezierCuadCurve curve(control,vectorPerp);
	largoCurva = curve.getSize();
	curve.getCurve(surface);
	curve.getNormals(normals);
	delete(control);
 }
}

/*void SweepSurf::createSurface(void) {
	surface = new vector<float>();
	normals = new vector<float>();
	glm::vec3 dir = glm::vec3(1.0,0.0,0.0);
	float angle=0;
	BezierCuadCurve curve(controlPoints,vectorPerp);
	largoCurva = curve.getSize(); // Calculo la longitud de la curva inicial.
	curve.getCurve(surface);
	curve.getNormals(normals);
	//Hasta aqui tengo el inicio de la superficie. Ahora debo recorrer el path.
	float x,y,z;
	for (unsigned int i = 3; i + 2 < path->size(); i+=3) {
		float incX,incY,incZ;
		incX = (*path)[i-3] - (*path)[i];
		incY = (*path)[i-2] - (*path)[i+1];
		incZ = (*path)[i-1] - (*path)[i+2];
		angle = anguloEntreVectores(dir, glm::vec3(incX, incY, incZ));
		rotateCtrlPointsFromOriginal(angle);
		vector<float>* auxiliar = new vector<float>();
		BezierCuadCurve curve(controlPoints,vectorPerp);
		curve.getCurve(auxiliar);
		curve.getNormals(normals);
		//Agrego cada punto de la nueva curva desplazada sobre el path
		x = (*path)[i]; y = (*path)[i+1]; z = (*path)[i+2];
		for (unsigned int j = 0; j + 2 < largoCurva; j+=3) {
			if (j+2 > auxiliar->size()) break;
			surface->push_back((*auxiliar)[j] + x);
			surface->push_back((*auxiliar)[j+1] + y);
			surface->push_back((*auxiliar)[j+2] + z);
		}//Hecho
		
		delete (auxiliar);
	}
	fprintf (stderr, "Fin creacion \n");
	
}*/

vector<float>* SweepSurf :: translate(){
 vector<float>* control = new  vector<float>();

 for (size_t i=0; i<controlPoints->size();i+=3){
	(*control).push_back((*controlPoints)[i]+delta[0]);
	(*control).push_back((*controlPoints)[i+1]+delta[1]);
	(*control).push_back((*controlPoints)[i+2]+delta[2]);
 }

 return control;
}

float SweepSurf :: calcularAngulo(float dx,float dy,float dz,float dx2,float dy2,float dz2){
	float norm  = sqrt(dx*dx + dy*dy + dz*dz);
	float norm2 = sqrt(dx2*dx2 + dy2*dy2 + dz2*dz2);
	float prod  = dx*dx2 + dy*dy2 + dz*dz2;

	return acos(prod/(norm*norm2));
}

void SweepSurf :: rotateCtrlPoints(float angle){
 glm::mat4 rot = rotate(angle);

 for(size_t i=0 ; i<controlPoints->size() ; i+=3){
	glm::vec4 Position = transform(rot,(*controlPoints)[i],(*controlPoints)[i+1],(*controlPoints)[i+2]);
	(*controlPoints)[i]   = Position[0];
	(*controlPoints)[i+1] = Position[1];
	(*controlPoints)[i+2] = Position[2];
 }
}

// transforms the position using the transformation matrix
glm::vec4 SweepSurf :: transform(glm::mat4 transfMat,float x, float y, float z){

  glm::vec4 Position = glm::vec4(x,y,z,1.0f);
  return Position*transfMat;
}

// rotation matrix (x axis)
glm::mat4 SweepSurf :: rotate(float ang){
  glm::mat4  Rx = glm::mat4(glm::vec4(1.0f,                0.0f,                  0.0f, 0.0f),
			    glm::vec4(0.0f,     (float)cos(ang),      (float)-sin(ang), 0.0f),
			    glm::vec4(0.0f,     (float)sin(ang),       (float)cos(ang), 0.0f),
			    glm::vec4(0.0f,                0.0f,                  0.0f, 1.0f));
 return Rx;
}


void SweepSurf :: drawSurface(glm::mat4 mat){
 glPointSize(1.0);
 glBegin(GL_TRIANGLES);
 for (float t=0; t + largoCurva + 5< surface->size() ;t+=3){	
		glm::vec4 Position = glm::vec4((*surface)[t],(*surface)[t+1],(*surface)[t+2],1.0f) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+3],(*surface)[t+4],(*surface)[t+5],1.0f) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+largoCurva],(*surface)[t+largoCurva+1],(*surface)[t+largoCurva+2],1.0f) * mat;
		glVertex3f(Position[0],Position[1],Position[2]);


		Position = glm::vec4((*surface)[t+largoCurva],(*surface)[t+largoCurva+1],(*surface)[t+largoCurva+2],1.0f) * mat;
		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+3],(*surface)[t+4],(*surface)[t+5],1.0f) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		Position = glm::vec4((*surface)[t+largoCurva+3],(*surface)[t+largoCurva+4],(*surface)[t+largoCurva+5],1.0f) * mat;
		glVertex3f(Position[0],Position[1],Position[2]);

 }
 glEnd();
}

void SweepSurf::createVertex(void) {
	float dens = 1;
	DatosVertice datos;
	size = (surface->size()/(3*dens));
	index_size = ((4*surface->size()/3)/dens);
	vertices = new DatosVertice[size];
	indices = new unsigned int[index_size];
	//Creo las estructuras.
	datos.color = glm::vec3(0.5,0.5,0.4);
	datos.tangent = glm::vec3(1.0,0.0,0.0);
	int contador = 0;
	int pos = 0;
	unsigned i = 0;
	for (i = 0; i + 5 < surface->size(); i += (dens*6), contador++) {
		float valorY = 0;
		if (contador >= largoCurva) {
			valorY = 1.0;
			contador = 0;
		}
		glm::vec3 posicion = glm::vec3((*surface)[i], (*surface)[i+1], (*surface)[i+2]);
		datos.position = posicion;
		datos.normal = glm::vec3((*normals)[i], (*normals)[i+1], (*normals)[i+2]);
		datos.texture= glm::vec2((float)0.0+contador/largoCurva, (float)valorY);
		vertices[pos++] = datos;
		contador++;
		datos.position = glm::vec3((*surface)[i+3], (*surface)[i+4], (*surface)[i+5]);
		datos.normal = glm::vec3((*normals)[i+3], (*normals)[i+4], (*normals)[i+5]);
		datos.texture = glm::vec2((float)1.0+contador/largoCurva,(float)valorY);
		vertices[pos++] = datos;
	}//Llene los vertices, ahora mediante el indice debo lograr que se formen los cuadrados.
	fprintf (stderr, "pos = %i, i = %i \n",pos,i);
	pos = 0;
	for (i = 0; i + largoCurva < size; i+=2 ) { // Aqui puedo establecer el tamanio de cada triangulo, variando el incremento.
		if (pos + 2 > index_size) {
			fprintf (stderr, "Error en la creacion del indice SweepSurf \n");
			break;
		}
		indices[pos++] = i;
		indices[pos++] = i + largoCurva;
	}
	this->type = GL_TRIANGLE_STRIP;
	index_size = pos;
	fprintf (stderr, "pos = %i  i = %i size = %i \n",pos, i, size);
	fprintf (stderr, "largoCurva = %ld \n",largoCurva);
	glGenBuffers(1, &vertex_buffer_object);
	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_object);
	glGenBuffers(1, &index_buffer_object);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffer_object);

	glBufferData(GL_ARRAY_BUFFER, size * sizeof(DatosVertice), vertices, GL_STATIC_DRAW);
	glBufferData(GL_ELEMENT_ARRAY_BUFFER, index_size * sizeof(unsigned int), indices, GL_STATIC_DRAW);

	glBindBuffer(GL_ARRAY_BUFFER, 0);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);

	delete (surface); delete(normals);

}

void SweepSurf::draw(GLuint program, glm::mat4 & world_matrix) {
	glm::mat4 matriz = world_matrix * this->model_matrix;

	GLuint location;
	// Normal Matrix es la inversa transpuesta 3x3 de gl_ModelViewMatrix (ViewMatrix = Camara)
	// Matrix = transformacion de afuera + transformacion del objeto

	calculate_and_bind_normal_matrix(program,matriz);
	bind_model_matrix(program,matriz);

	/*glEnableClientState(GL_VERTEX_ARRAY);
	 glEnableClientState(GL_NORMAL_ARRAY);
	 glEnableClientState(GL_TEXTURE_COORD_ARRAY);
	 glEnable(GL_CULL_FACE);



	 bind_tangent_buffer("tangent");

	 glVertexPointer(3, GL_FLOAT, 0, vertex_buffer);
	 glNormalPointer(GL_FLOAT, 0, normal_buffer);
	 glTexCoordPointer(2, GL_FLOAT, 0, texture_buffer);

	 glDrawElements(GL_QUADS, index_buffer_size, GL_UNSIGNED_INT, index_buffer);

	 if (this->show_normals) {
	 draw_normals();
	 }

	 glDisableClientState(GL_VERTEX_ARRAY);
	 glDisableClientState(GL_NORMAL_ARRAY);
	 glDisableClientState(GL_TEXTURE_COORD_ARRAY);*/

	int vertex = glGetAttribLocation(program, "vertex"); //0
	int normal = glGetAttribLocation(program, "normal"); //1
	int tangent = glGetAttribLocation(program, "tangent"); //2
	int color = glGetAttribLocation(program, "color"); //3
	int UV = glGetAttribLocation(program, "UV"); //4

	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_object);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffer_object);

	glEnableVertexAttribArray(vertex);
	glVertexAttribPointer(vertex, 3, GL_FLOAT, GL_FALSE, sizeof(DatosVertice), 0);

	glEnableVertexAttribArray(normal);
	glVertexAttribPointer(normal, 3, GL_FLOAT, GL_FALSE, sizeof(DatosVertice), (void*) (3 * sizeof(float)));

	glEnableVertexAttribArray(tangent);
	glVertexAttribPointer(tangent, 3, GL_FLOAT, GL_FALSE, sizeof(DatosVertice), (void*) (6 * sizeof(float)));

	glEnableVertexAttribArray(color);
	glVertexAttribPointer(color, 3, GL_FLOAT, GL_FALSE, sizeof(DatosVertice), (void*) (9 * sizeof(float)));

	glEnableVertexAttribArray(UV);
	glVertexAttribPointer(UV, 2, GL_FLOAT, GL_FALSE, sizeof(DatosVertice), (void*) (12 * sizeof(float)));

	// TEXTURE2D
	if (with_texture) {

		glActiveTexture(GL_TEXTURE0);
		glBindTexture(GL_TEXTURE_2D, this->texture);
		location = glGetUniformLocation(program, "Texture");
		if (location >= 0) {
			glUniform1i(location, 0);
		}
		location = glGetUniformLocation(program, "with_texture");
		if (location >= 0) {
			glUniform1i(location, 1);
		}
	} else {
		location = glGetUniformLocation(program, "with_texture");
		if (location >= 0) {
			glUniform1i(location, 0);
		}
	}

	glDrawElements(this->type, index_size, GL_UNSIGNED_INT, 0);

	glDisableVertexAttribArray(vertex);
	glDisableVertexAttribArray(normal);
	glDisableVertexAttribArray(tangent);
	glDisableVertexAttribArray(color);
	glDisableVertexAttribArray(UV);

	if (with_texture) {
		glBindTexture(GL_TEXTURE_2D, 0);
	}

	glBindBuffer(GL_ARRAY_BUFFER, 0);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
}


void SweepSurf :: drawNormals(glm::mat4 mat){
 
 for (size_t i=0 ; i<surface->size()-3 ; i+=3){
		glBegin(GL_LINES);

		glm::vec4 Position = glm::vec4((*surface)[i],(*surface)[i+1],(*surface)[i+2],1.0) * mat;
 		glVertex3f(Position[0],Position[1],Position[2]);

		glm::vec4 Position2 = glm::vec4((*surface)[i]+(*normals)[i],(*surface)[i+1]+(*normals)[i+1],(*surface)[i+2]+(*normals)[i+2],1.0) * mat;
 		glVertex3f(Position2[0],Position2[1],Position2[2]);
	
		 glEnd();

		if( (i%largoCurva) == 0)
			i+= largoCurva*DETAILNORMALSSWEEPSURF;
  }
}

void SweepSurf :: drawCurve(glm::mat4 mat){
 BezierCuadCurve curve(controlPoints,vectorPerp);
 curve.drawCurve(mat);
 curve.drawNormals();
}

void SweepSurf :: drawPath(glm::mat4 mat){
 BezierCuadCurve curve(path,vectorPerp);
 curve.drawCurve(mat);
}

void SweepSurf :: drawControlPoints(){
 glBegin(GL_LINE_STRIP);
 for (float t=0; t< controlPoints->size() ;t+=3)	
		glVertex3f((*controlPoints)[t],(*controlPoints)[t+1],(*controlPoints)[t+2]);
 glEnd();
}

void SweepSurf::rotateCtrlPointsFromOriginal(float angle) {
	//Rota los puntos de contro desde su posición original.
	glm::mat4 original = glm::mat4(1.0f);
	glm::mat4 rot = glm::rotate(original, angle,glm::vec3(0,0,1));

	 for(size_t i=0 ; i < originalControlPoints->size() ; i+=3){
		glm::vec4 Position = glm::vec4((*originalControlPoints)[i],(*originalControlPoints)[i+1],(*originalControlPoints)[i+2],1.0) * rot;
		(*controlPoints)[i]   = Position[0];
		(*controlPoints)[i+1] = Position[1];
		(*controlPoints)[i+2] = Position[2];
	 }
}
